/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;

import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.tables.ITableListener;
import java.util.Enumeration;
import java.util.NoSuchElementException;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.NamedSendable;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.networktables2.util.Set;

/**
 * The Command class is at the very core of the entire command framework. Every
 * command can be started with a call to {@link Command#start() start()}. Once a
 * command is started it will call {@link Command#initialize() initialize()},
 * and then will repeatedly call {@link Command#execute() execute()} until the
 * {@link Command#isFinished() isFinished()} returns true. Once it does,
 * {@link Command#end() end()} will be called.
 *
 * <p>However, if at any point while it is running
 * {@link Command#cancel() cancel()} is called, then the command will be stopped
 * and {@link Command#interrupted() interrupted()} will be called.</p>
 *
 * <p>If a command uses a {@link Subsystem}, then it should specify that it does
 * so by calling the {@link Command#requires(Subsystem) requires(...)} method in
 * its constructor. Note that a Command may have multiple requirements, and
 * {@link Command#requires(Subsystem) requires(...)} should be called for each
 * one.</p>
 *
 * <p>If a command is running and a new command with shared requirements is
 * started, then one of two things will happen. If the active command is
 * interruptible, then {@link Command#cancel() cancel()} will be called and the
 * command will be removed to make way for the new one. If the active command is
 * not interruptible, the other one will not even be started, and the active one
 * will continue functioning.</p>
 *
 * @author Brad Miller
 * @author Joe Grinstead
 * @see Subsystem
 * @see CommandGroup
 * @see IllegalUseOfCommandException
 */
public abstract class Command implements NamedSendable {

    /**
     * The name of this command
     */
    private String m_name;
    /**
     * The time since this command was initialized
     */
    private double m_startTime = -1;
    /**
     * The time (in seconds) before this command "times out" (or -1 if no
     * timeout)
     */
    private double m_timeout = -1;
    /**
     * Whether or not this command has been initialized
     */
    private boolean m_initialized = false;
    /**
     * The requirements (or null if no requirements)
     */
    private Set m_requirements;
    /**
     * Whether or not it is running
     */
    private boolean m_running = false;
    /**
     * Whether or not it is interruptible
     */
    private boolean m_interruptible = true;
    /**
     * Whether or not it has been canceled
     */
    private boolean m_canceled = false;
    /**
     * Whether or not it has been locked
     */
    private boolean m_locked = false;
    /**
     * Whether this command should run when the robot is disabled
     */
    private boolean m_runWhenDisabled = false;
    /**
     * The {@link CommandGroup} this is in
     */
    private CommandGroup m_parent;

    /**
     * Creates a new command. The name of this command will be set to its class
     * name.
     */
    public Command() {
        m_name = getClass().getName();
        m_name = m_name.substring(m_name.lastIndexOf('.') + 1);
    }

    /**
     * Creates a new command with the given name.
     *
     * @param name the name for this command
     * @throws IllegalArgumentException if name is null
     */
    public Command(String name) {
        if (name == null) {
            throw new IllegalArgumentException("Name must not be null.");
        }
        m_name = name;
    }

    /**
     * Creates a new command with the given timeout and a default name. The
     * default name is the name of the class.
     *
     * @param timeout the time (in seconds) before this command "times out"
     * @throws IllegalArgumentException if given a negative timeout
     * @see Command#isTimedOut() isTimedOut()
     */
    public Command(double timeout) {
        this();
        if (timeout < 0) {
            throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
        }
        m_timeout = timeout;
    }

    /**
     * Creates a new command with the given name and timeout.
     *
     * @param name the name of the command
     * @param timeout the time (in seconds) before this command "times out"
     * @throws IllegalArgumentException if given a negative timeout or name was
     * null.
     * @see Command#isTimedOut() isTimedOut()
     */
    public Command(String name, double timeout) {
        this(name);
        if (timeout < 0) {
            throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
        }
        m_timeout = timeout;
    }

    /**
     * Returns the name of this command. If no name was specified in the
     * constructor, then the default is the name of the class.
     *
     * @return the name of this command
     */
    public String getName() {
        return m_name;
    }

    /**
     * Sets the timeout of this command.
     *
     * @param seconds the timeout (in seconds)
     * @throws IllegalArgumentException if seconds is negative
     * @see Command#isTimedOut() isTimedOut()
     */
    protected synchronized final void setTimeout(double seconds) {
        if (seconds < 0) {
            throw new IllegalArgumentException("Seconds must be positive.  Given:" + seconds);
        }
        m_timeout = seconds;
    }

    /**
     * Returns the time since this command was initialized (in seconds). This
     * function will work even if there is no specified timeout.
     *
     * @return the time since this command was initialized (in seconds).
     */
    public synchronized final double timeSinceInitialized() {
        return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
    }

    /**
     * This method specifies that the given {@link Subsystem} is used by this
     * command. This method is crucial to the functioning of the Command System
     * in general.
     *
     * <p>Note that the recommended way to call this method is in the
     * constructor.</p>
     *
     * @param subsystem the {@link Subsystem} required
     * @throws IllegalArgumentException if subsystem is null
     * @throws IllegalUseOfCommandException if this command has started before
     * or if it has been given to a {@link CommandGroup}
     * @see Subsystem
     */
    protected synchronized void requires(Subsystem subsystem) {
        validate("Can not add new requirement to command");
        if (subsystem != null) {
            if (m_requirements == null) {
                m_requirements = new Set();
            }
            m_requirements.add(subsystem);
        } else {
            throw new IllegalArgumentException("Subsystem must not be null.");
        }
    }

    /**
     * Called when the command has been removed. This will call
     * {@link Command#interrupted() interrupted()} or
     * {@link Command#end() end()}.
     */
    synchronized void removed() {
        if (m_initialized) {
            if (isCanceled()) {
                interrupted();
                _interrupted();
            } else {
                end();
                _end();
            }
        }
        m_initialized = false;
        m_canceled = false;
        m_running = false;
        if (table != null) {
            table.putBoolean("running", false);
        }
    }

    /**
     * The run method is used internally to actually run the commands.
     *
     * @return whether or not the command should stay within the
     * {@link Scheduler}.
     */
    synchronized boolean run() {
        if (!m_runWhenDisabled && m_parent == null && DriverStation.getInstance().isDisabled()) {
            cancel();
        }
        if (isCanceled()) {
            return false;
        }
        if (!m_initialized) {
            m_initialized = true;
            startTiming();
            _initialize();
            initialize();
        }
        _execute();
        execute();
        return !isFinished();
    }

    /**
     * The initialize method is called the first time this Command is run after
     * being started.
     */
    protected abstract void initialize();

    /**
     * A shadow method called before {@link Command#initialize() initialize()}
     */
    void _initialize() {
    }

    /**
     * The execute method is called repeatedly until this Command either
     * finishes or is canceled.
     */
    protected abstract void execute();

    /**
     * A shadow method called before {@link Command#execute() execute()}
     */
    void _execute() {
    }

    /**
     * Returns whether this command is finished. If it is, then the command will
     * be removed and {@link Command#end() end()} will be called.
     *
     * <p>It may be useful for a team to reference the
     * {@link Command#isTimedOut() isTimedOut()} method for time-sensitive
     * commands.</p>
     *
     * @return whether this command is finished.
     * @see Command#isTimedOut() isTimedOut()
     */
    protected abstract boolean isFinished();

    /**
     * Called when the command ended peacefully. This is where you may want to
     * wrap up loose ends, like shutting off a motor that was being used in the
     * command.
     */
    protected abstract void end();

    /**
     * A shadow method called after {@link Command#end() end()}.
     */
    void _end() {
    }

    /**
     * Called when the command ends because somebody called
     * {@link Command#cancel() cancel()} or another command shared the same
     * requirements as this one, and booted it out.
     *
     * <p>This is where you may want to wrap up loose ends, like shutting off a
     * motor that was being used in the command.</p>
     *
     * <p>Generally, it is useful to simply call the {@link Command#end() end()}
     * method within this method</p>
     */
    protected abstract void interrupted();

    /**
     * A shadow method called after {@link Command#interrupted() interrupted()}.
     */
    void _interrupted() {
    }

    /**
     * Called to indicate that the timer should start. This is called right
     * before {@link Command#initialize() initialize()} is, inside the
     * {@link Command#run() run()} method.
     */
    private void startTiming() {
        m_startTime = Timer.getFPGATimestamp();
    }

    /**
     * Returns whether or not the
     * {@link Command#timeSinceInitialized() timeSinceInitialized()} method
     * returns a number which is greater than or equal to the timeout for the
     * command. If there is no timeout, this will always return false.
     *
     * @return whether the time has expired
     */
    protected synchronized boolean isTimedOut() {
        return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
    }

    /**
     * Returns the requirements (as an {@link Enumeration Enumeration} of
     * {@link Subsystem Subsystems}) of this command
     *
     * @return the requirements (as an {@link Enumeration Enumeration} of
     * {@link Subsystem Subsystems}) of this command
     */
    synchronized Enumeration getRequirements() {
        return m_requirements == null ? emptyEnumeration : m_requirements.getElements();

    }

    /**
     * Prevents further changes from being made
     */
    synchronized void lockChanges() {
        m_locked = true;
    }

    /**
     * If changes are locked, then this will throw an
     * {@link IllegalUseOfCommandException}.
     *
     * @param message the message to say (it is appended by a default message)
     */
    synchronized void validate(String message) {
        if (m_locked) {
            throw new IllegalUseOfCommandException(message + " after being started or being added to a command group");
        }
    }

    /**
     * Sets the parent of this command. No actual change is made to the group.
     *
     * @param parent the parent
     * @throws IllegalUseOfCommandException if this {@link Command} already is
     * already in a group
     */
    synchronized void setParent(CommandGroup parent) {
        if (this.m_parent != null) {
            throw new IllegalUseOfCommandException("Can not give command to a command group after already being put in a command group");
        }
        lockChanges();
        this.m_parent = parent;
        if (table != null) {
            table.putBoolean("isParented", true);
        }
    }

    /**
     * Starts up the command. Gets the command ready to start.
     * <p>Note that the command will eventually start, however it will not
     * necessarily do so immediately, and may in fact be canceled before
     * initialize is even called.</p>
     *
     * @throws IllegalUseOfCommandException if the command is a part of a
     * CommandGroup
     */
    public synchronized void start() {
        lockChanges();
        if (m_parent != null) {
            throw new IllegalUseOfCommandException("Can not start a command that is a part of a command group");
        }
        Scheduler.getInstance().add(this);
    }

    /**
     * This is used internally to mark that the command has been started. The
     * lifecycle of a command is:
     *
     * startRunning() is called. run() is called (multiple times potentially)
     * removed() is called
     *
     * It is very important that startRunning and removed be called in order or
     * some assumptions of the code will be broken.
     */
    synchronized void startRunning() {
        m_running = true;
        m_startTime = -1;
        if (table != null) {
            table.putBoolean("running", true);
        }
    }

    /**
     * Returns whether or not the command is running. This may return true even
     * if the command has just been canceled, as it may not have yet called
     * {@link Command#interrupted()}.
     *
     * @return whether or not the command is running
     */
    public synchronized boolean isRunning() {
        return m_running;
    }

    /**
     * This will cancel the current command.
     * <p>This will cancel the current command eventually. It can be called
     * multiple times. And it can be called when the command is not running. If
     * the command is running though, then the command will be marked as
     * canceled and eventually removed.</p>
     * <p>A command can not be canceled if it is a part of a command group, you
     * must cancel the command group instead.</p>
     *
     * @throws IllegalUseOfCommandException if this command is a part of a
     * command group
     */
    public synchronized void cancel() {
        if (m_parent != null) {
            throw new IllegalUseOfCommandException("Can not manually cancel a command in a command group");
        }
        _cancel();
    }

    /**
     * This works like cancel(), except that it doesn't throw an exception if it
     * is a part of a command group. Should only be called by the parent command
     * group.
     */
    synchronized void _cancel() {
        if (isRunning()) {
            m_canceled = true;
        }
    }

    /**
     * Returns whether or not this has been canceled.
     *
     * @return whether or not this has been canceled
     */
    public synchronized boolean isCanceled() {
        return m_canceled;
    }

    /**
     * Returns whether or not this command can be interrupted.
     *
     * @return whether or not this command can be interrupted
     */
    public synchronized boolean isInterruptible() {
        return m_interruptible;
    }

    /**
     * Sets whether or not this command can be interrupted.
     *
     * @param interruptible whether or not this command can be interrupted
     */
    protected synchronized void setInterruptible(boolean interruptible) {
        this.m_interruptible = interruptible;
    }

    /**
     * Checks if the command requires the given {@link Subsystem}.
     *
     * @param system the system
     * @return whether or not the subsystem is required, or false if given null
     */
    public synchronized boolean doesRequire(Subsystem system) {
        return m_requirements != null && m_requirements.contains(system);
    }

    /**
     * Returns the {@link CommandGroup} that this command is a part of. Will
     * return null if this {@link Command} is not in a group.
     *
     * @return the {@link CommandGroup} that this command is a part of (or null
     * if not in group)
     */
    public synchronized CommandGroup getGroup() {
        return m_parent;
    }

    /**
     * Sets whether or not this {@link Command} should run when the robot is
     * disabled.
     *
     * <p>By default a command will not run when the robot is disabled, and will
     * in fact be canceled.</p>
     *
     * @param run whether or not this command should run when the robot is
     * disabled
     */
    public void setRunWhenDisabled(boolean run) {
        m_runWhenDisabled = run;
    }

    /**
     * Returns whether or not this {@link Command} will run when the robot is
     * disabled, or if it will cancel itself.
     *
     * @return whether or not this {@link Command} will run when the robot is
     * disabled, or if it will cancel itself
     */
    public boolean willRunWhenDisabled() {
        return m_runWhenDisabled;
    }
    /**
     * An empty enumeration given whenever there are no requirements
     */
    private static Enumeration emptyEnumeration = new Enumeration() {
        public boolean hasMoreElements() {
            return false;
        }

        public Object nextElement() {
            throw new NoSuchElementException();
        }
    };

    /**
     * The string representation for a {@link Command} is by default its name.
     *
     * @return the string representation of this object
     */
    public String toString() {
        return getName();
    }

    public String getSmartDashboardType() {
        return "Command";
    }
    private ITableListener listener = new ITableListener() {
        public void valueChanged(ITable table, String key, Object value, boolean isNew) {
            if (((Boolean) value).booleanValue()) {
                start();
            } else {
                cancel();
            }
        }
    };
    private ITable table;

    public void initTable(ITable table) {
        if (this.table != null) {
            this.table.removeTableListener(listener);
        }
        this.table = table;
        if (table != null) {
            table.putString("name", getName());
            table.putBoolean("running", isRunning());
            table.putBoolean("isParented", m_parent != null);
            table.addTableListener("running", listener, false);
        }
    }

    /**
     * {@inheritDoc}
     */
    public ITable getTable() {
        return table;
    }

    
}
